#include "recoloring.h"

#include"recoloring.h"

Recoloring::Recoloring()
{

}
void Recoloring::init(Mat img)
{
    img.copyTo(this->imaging);
    cv::cvtColor(this->imaging, this->imaging, CV_BGR2RGB);
    this->onload();
    this->center = this->k_means(this->imaging, centersize);


    this->IndexSortOrder=new int[5];
    double*input_L = new double[5];
    double*tmp=new double[3];
    for(int i=0;i < 5;i++ )
    {
        tmp[0]=this->center[3*i]*255.0;
        tmp[1]=this->center[3*i+1]*255.0;
        tmp[2]=this->center[3*i+2]*255.0;
        IndexSortOrder[i] = i;
        input_L[i] = this->RGB2LAB( tmp)[0];
    }
    for(int i=0;i < 5; i++ )
        for(int j=i+1;	j < 5; j++ )
            if( input_L[IndexSortOrder[i]] > input_L[IndexSortOrder[j]] )
            {
                double tmp = IndexSortOrder[i];
                IndexSortOrder[i] = IndexSortOrder[j];
                IndexSortOrder[j] = tmp;
            }
    double* tmp_palette = new double[15];
    for(int i=0; i<15; i++)
    {
        tmp_palette[i] = this->center[i];
    }
    for(int i=0; i<5; i++)
    {
        int j = IndexSortOrder[i];
        this->center[3*i] 		= tmp_palette[3*j];
        this->center[3*i+1] 	= tmp_palette[3*j+1];
        this->center[3*i+2] 	= tmp_palette[3*j+2];
    }

    this->changed_center=new double[15];
    for(int i=0;i<15;++i)
        this->changed_center[i]=this->center[i];
}

double* Recoloring::RGB2LAB(double*Q)
{
    //alert("R2L "+Q);
    double var_R = (Q[0] / 255);        //R from 0 to 255
    double var_G = (Q[1] / 255);        //G from 0 to 255
    double var_B = (Q[2] / 255);        //B from 0 to 255


    if (var_R > 0.04045) var_R = std::pow((var_R + 0.055) / 1.055, 2.4);
    else                   var_R = var_R / 12.92;

    if (var_G > 0.04045) var_G = std::pow((var_G + 0.055) / 1.055, 2.4);
    else                   var_G = var_G / 12.92;

    if (var_B > 0.04045) var_B = std::pow((var_B + 0.055) / 1.055, 2.4);
    else                   var_B = var_B / 12.92;

    var_R = var_R * 100.0;
    var_G = var_G * 100.0;
    var_B = var_B * 100.0;

    //Observer. = 2°, Illuminant = D65
    double X = var_R * 0.4124 + var_G * 0.3576 + var_B * 0.1805;
    double Y = var_R * 0.2126 + var_G * 0.7152 + var_B * 0.0722;
    double Z = var_R * 0.0193 + var_G * 0.1192 + var_B * 0.9505;

    double var_X = X / 95.047;
    double var_Y = Y / 100.0;
    double var_Z = Z / 108.883;

    if (var_X > 0.008856) var_X = std::pow(var_X, double(1.0 / 3.0));
    else                    var_X = (7.787 * var_X) + (16.0 / 116.0);

    if (var_Y > 0.008856) var_Y = std::pow(var_Y, double(1.0 / 3.0));
    else                    var_Y = (7.787 * var_Y) + (16.0 / 116.0);

    if (var_Z > 0.008856) var_Z = std::pow(var_Z, double(1.0 / 3.0));
    else                    var_Z = (7.787 * var_Z) + (16.0 / 116.0);

    double L = (116.0 * var_Y) - 16.0;
    double A = 500.0 * (var_X - var_Y);
    double B = 200.0* (var_Y - var_Z);
    double*ret = new double[3];
    ret[0] = L;
    ret[1] = A;
    ret[2] = B;
    return ret;
}
double* Recoloring::LAB2RGB(double*Q)
{
    //alert("L2R "+Q);
    double var_Y = (Q[0] + 16.0) / 116.0;
    double var_X = Q[1] / 500.0 + var_Y;
    double var_Z = var_Y - Q[2] / 200.0;

    if (var_Y > 0.206893034422) var_Y = std::pow(var_Y, 3.0);
    else                      var_Y = (var_Y - 16.0 / 116.0) / 7.787;
    if (var_X > 0.206893034422) var_X = std::pow(var_X, 3.0);
    else                      var_X = (var_X - 16.0 / 116.0) / 7.787;
    if (var_Z > 0.206893034422) var_Z = std::pow(var_Z, 3.0);
    else                      var_Z = (var_Z - 16.0 / 116.0) / 7.787;

    double X = 95.047 * var_X;
    double Y = 100.0 * var_Y;
    double Z = 108.883 * var_Z;

    var_X = X / 100.0;
    var_Y = Y / 100.0;
    var_Z = Z / 100.0;

    double var_R = var_X *  3.2406 + var_Y * -1.5372 + var_Z * -0.4986;
    double var_G = var_X * -0.9689 + var_Y *  1.8758 + var_Z *  0.0415;
    double var_B = var_X *  0.0557 + var_Y * -0.2040 + var_Z *  1.0570;

    if (var_R > 0.0031308) var_R = 1.055 * std::pow(var_R, 1.0 / 2.4) - 0.055;
    else                     var_R = 12.92 * var_R;
    if (var_G > 0.0031308) var_G = 1.055 * std::pow(var_G, 1.0 / 2.4) - 0.055;
    else                     var_G = 12.92 * var_G;
    if (var_B > 0.0031308) var_B = 1.055 * std::pow(var_B, 1.0 / 2.4) - 0.055;
    else                     var_B = 12.92 * var_B;

    double R = var_R * 255.0;
    double G = var_G * 255.0;
    double B = var_B * 255.0;
    double*ret = new double[3];
    ret[0] = R;
    ret[1] = G;
    ret[2] = B;
    return ret;
}
double* Recoloring::Multi(double**A, int ma, double*B)
{
    Mat MA(ma, ma, CV_64F);
    Mat MB(ma, 1, CV_64F);
    Mat MC(ma, 1, CV_64F);
    for (int i = 0; i<MA.rows; i++)
        for (int j = 0; j<MA.cols; j++)
            MA.at<double>(i, j) = A[i][j];
    MA = MA.inv();
    for (int i = 0; i<MB.rows; i++)
        MB.at<double>(i, 0) = B[i];
    MC = MA*MB;
    double *ret = new double[ma];
    for (int i = 0; i < ma; ++i)
    {
        ret[i] = MC.at<double>(i, 0);
    }
    return ret;
}
void Recoloring::onload()
{
    int imageSize = imaging.rows*imaging.cols;
    this->ori_b = new double[imageSize];
    this->ori_g = new double[imageSize];
    this->ori_r = new double[imageSize];
    int cnt = 0;
    for (int i = 0; i < imaging.rows; i++)
        for (int j = 0; j < imaging.cols; j++) {
            Vec3b pix_bgr = imaging.ptr<Vec3b>(i)[j];
            this->ori_r[cnt] = pix_bgr.val[0];
            this->ori_g[cnt] = pix_bgr.val[1];
            this->ori_b[cnt] = pix_bgr.val[2];
            cnt = cnt + 1;
        }
    this->prepare_grid();

    this->res_b = new double[imageSize];
    this->res_g = new double[imageSize];
    this->res_r = new double[imageSize];


    this->mask = new double[imageSize];
    for (int i = 0; i < imageSize; i++)
    {
        this->mask[i] = 1.0;
    }

}
void Recoloring::prepare_grid()
{
    int imageSize = imaging.rows*imaging.cols;

    this->grid_size = (ngrid + 1) * (ngrid + 1) * (ngrid + 1);
    this->grid_lab = new double*[this->grid_size];
    double step = 255.0 / ngrid;
    int tot = 0;
    double*trans = new double[3];
    for (int i = 0; i<ngrid + 1; i++)
        for (int j = 0; j<ngrid + 1; j++)
            for (int k = 0; k<ngrid + 1; k++)
            {
                trans[0] = k*step;
                trans[1] = j*step;
                trans[2] = i*step;
                this->grid_lab[tot] = RGB2LAB(trans);
                tot = tot + 1;
            }

    // calculate the weight index and map for image pixel
    this->weightindex = new int*[imageSize];
    this->weightmap = new double*[imageSize];
    for (int i = 0; i < imageSize; ++i){
        this->weightmap[i] = new double[8];
        this->weightindex[i] = new int[8];
    }
    for (int i = 0; i<imageSize; i++) {
        double* res = this->calculate_singlegrid(
            this->ori_r[i], this->ori_g[i], this->ori_b[i]);
        for (int j = 0; j < 8; j++)
        {
            this->weightindex[i][j] = res[j];
        }

        for (int j = 0; j < 8; j++)
        {
            this->weightmap[i][j] = res[j + 8];
        }
    }
    // initialize result grid
    this->grid_R = new double[imageSize];
    this->grid_G = new double[imageSize];
    this->grid_B = new double[imageSize];
}
double* Recoloring::calculate_singlegrid(double ori_r, double ori_g, double ori_b)
{
    double ntmp = ngrid + 1;
    double ntmpsqr = ntmp*ntmp;
    double diff_x, diff_y, diff_z;
    double corner_ind;
    double tmpx, tmpy, tmpz;

    tmpx = ori_r / 255.0 * ngrid;
    diff_x = tmpx - floor(tmpx);
    tmpx = floor(tmpx);
    if (tmpx == ngrid){
        tmpx = ngrid - 1;
        diff_x = 1;
    }
    tmpy = ori_g / 255.0 * ngrid;
    diff_y = tmpy - floor(tmpy);
    tmpy = floor(tmpy);
    if (tmpy == ngrid){
        tmpy = ngrid - 1;
        diff_y = 1;
    }
    tmpz = ori_b / 255.0 * ngrid;
    diff_z = tmpz - floor(tmpz);
    tmpz = floor(tmpz);
    if (tmpz == ngrid){
        tmpz = ngrid - 1;
        diff_z = 1;
    }

    corner_ind = tmpz * ntmpsqr + tmpy * ntmp + tmpx;

    double* res = new double[16];

    res[0] = corner_ind;
    res[1] = corner_ind + ntmpsqr;
    res[2] = corner_ind + ntmp;
    res[3] = corner_ind + ntmp + ntmpsqr;
    res[4] = corner_ind + 1;
    res[5] = corner_ind + ntmpsqr + 1;
    res[6] = corner_ind + ntmp + 1;
    res[7] = corner_ind + ntmp + ntmpsqr + 1;

    res[8] = (1 - diff_x)*(1 - diff_y)*(1 - diff_z);
    res[9] = (1 - diff_x)*(1 - diff_y)*diff_z;
    res[10] = (1 - diff_x)*diff_y*(1 - diff_z);
    res[11] = (1 - diff_x)*diff_y*diff_z;
    res[12] = diff_x*(1 - diff_y)*(1 - diff_z);
    res[13] = diff_x*(1 - diff_y)*diff_z;
    res[14] = diff_x*diff_y*(1 - diff_z);
    res[15] = diff_x*diff_y*diff_z;

    return res;
}
double Recoloring::CalculateLABDistance(
    double l1, double a1, double b1,
    double l2, double a2, double b2)
{
    double K1 = 0.045, K2 = 0.015;
    double del_L = l1 - l2;
    double c1 = sqrt(a1*a1 + b1*b1);
    double c2 = sqrt(a2*a2 + b2*b2);
    double c_ab = c1 - c2;
    double h_ab = (a1 - a2)*(a1 - a2) + (b1 - b2)*(b1 - b2) - c_ab*c_ab;
    return del_L*del_L + c_ab *c_ab / (1 + K1*c1) / (1 + K1*c1) + h_ab / (1 + K2*c1) / (1 + K2*c1);

}
bool Recoloring::Bigchange(double*dir)
{
    return (abs(dir[0])>0.5 || abs(dir[1])>0.5 || abs(dir[2])>0.5);
}
bool Recoloring::OutBoundary(double*testrgb)
{
    double out_threshold = 0.5;
    return (testrgb[0] < -out_threshold || testrgb[0] > 255.0 + out_threshold ||
        testrgb[1] < -out_threshold || testrgb[1] > 255.0 + out_threshold ||
        testrgb[2] < -out_threshold || testrgb[2] > 255.0 + out_threshold);
}
double Recoloring::FindBoundary(
    double*vsrc, double* dir, double l, double r)
{
    //Assume dir is large
    double mid;
    for (int iter = 0; iter < 15; iter++)
    {
        mid = 0.5 * (l + r);
        double temp[3];
        temp[0] = vsrc[0] + mid * dir[0];
        temp[1] = vsrc[1] + mid * dir[1];
        temp[2] = vsrc[2] + mid * dir[2];
        double* testrgb = LAB2RGB(temp);
        if (OutBoundary(testrgb))
        {
            r = mid;
        }
        else
        {
            l = mid;
        }
    }
    return l;
}
double *Recoloring::CalculateSinglePoint(double param, int matrixsize,
    double*oldpalette_L, double*oldpalette_A, double* oldpalette_B,
    double*diffpalette_L, double*diffpalette_A, double*diffpalette_B, double*vsrc)
{
    // tmpMat: e^oldpalette distance
    double** tmpMat = new double*[matrixsize];
    for (int i = 0; i < matrixsize; i++)
        tmpMat[i] = new double[matrixsize];

    for (int u = 0; u < matrixsize; u++)
        for (int v = 0; v < matrixsize; v++)
        {
            double r = CalculateLABDistance(oldpalette_L[u], oldpalette_A[u], oldpalette_B[u], oldpalette_L[v], oldpalette_A[v], oldpalette_B[v]);
            tmpMat[u][v] = exp(-r*param);
        }
    // tmpD: e^distance(vsrc , oldpalette)
    double *tmpD = new double[matrixsize];
    for (int u = 0; u < matrixsize; u++)
    {
        double r = CalculateLABDistance(oldpalette_L[u], oldpalette_A[u], oldpalette_B[u], vsrc[0], vsrc[1], vsrc[2]);
        tmpD[u] = exp(-r*param);
    }

    double*precompute_pinv = new double[matrixsize];
    precompute_pinv = Multi(tmpMat, matrixsize, tmpD);

    double delta_L = 0.0;
    double delta_A = 0.0;
    double delta_B = 0.0;

    double scale = 0.0;
    for (int j = 0; j < matrixsize; j++)
    {
        scale = scale + max(precompute_pinv[j], 0.0);
    }
    //if( scale < 1){ scale = 1.0;}
    for (int j = 0; j < matrixsize; j++)
        if (precompute_pinv[j]>0)
        {
            delta_L = delta_L + precompute_pinv[j] / scale * diffpalette_L[j];
            delta_A = delta_A + precompute_pinv[j] / scale * diffpalette_A[j];
            delta_B = delta_B + precompute_pinv[j] / scale * diffpalette_B[j];
        }
    double*ret = new double[3];
    ret[0] = vsrc[0] + delta_L;
    ret[1] = vsrc[1] + delta_A;
    ret[2] = vsrc[2] + delta_B;
    return ret;
}
void Recoloring::CalculateGridResult(int palette_size,
    double*oldpalette_L, double*oldpalette_A, double*oldpalette_B,
    double*diffpalette_L, double*diffpalette_A, double*diffpalette_B)
{
    //average distance
    double RBF_param_coff = 5.0;
    double tot = 0;
    double totr = 0;
    for (int u = 0; u < palette_size; u++)
        for (int v = u + 1; v < palette_size; v++)
        {
            double r = CalculateLABDistance(oldpalette_L[u], oldpalette_A[u], oldpalette_B[u],
                oldpalette_L[v], oldpalette_A[v], oldpalette_B[v]);
            tot = tot + 1.0;
            totr = totr + sqrt(r);
        }

    if (palette_size > 1)
    {
        totr = totr / tot;
    }
    else
    {
        totr = 1.0;
    }

    double param = RBF_param_coff / (totr*totr);

    for (int i = 0; i < grid_size; i++)
    {
        double* vsrc = grid_lab[i];

        double* tdiff_L = new double[palette_size];
        double* tdiff_A = new double[palette_size];
        double* tdiff_B = new double[palette_size];

        for (int j = 0; j < palette_size; j++)
        {
            double*dir = new double[3];
            dir[0] = diffpalette_L[j];
            dir[1] = diffpalette_A[j];
            dir[2] = diffpalette_B[j];

            if (Bigchange(dir))
            {
                double tmpColor[3];
                tmpColor[0] = vsrc[0] + dir[0];
                tmpColor[1] = vsrc[1] + dir[1];
                tmpColor[2] = vsrc[2] + dir[2];
                double* pc = LAB2RGB(tmpColor);
                if (OutBoundary(pc))
                {
                    double M[3];
                    M[0] = oldpalette_L[j] + dir[0];
                    M[1] = oldpalette_A[j] + dir[1];
                    M[2] = oldpalette_B[j] + dir[2];
                    double Mdir[3];
                    Mdir[0] = vsrc[0] - oldpalette_L[j];
                    Mdir[1] = vsrc[1] - oldpalette_A[j];
                    Mdir[2] = vsrc[2] - oldpalette_B[j];

                    double t1 = FindBoundary(M, Mdir, 0, 1);

                    double tmpPalette[3];
                    tmpPalette[0] = oldpalette_L[j];
                    tmpPalette[1] = oldpalette_A[j];
                    tmpPalette[2] = oldpalette_B[j];
                    double t2 = FindBoundary(tmpPalette, dir, 1, 300);

                    tdiff_L[j] = (dir[0] - (1 - t1)*Mdir[0]);/// t2;
                    tdiff_A[j] = (dir[1] - (1 - t1)*Mdir[1]) / t2;
                    tdiff_B[j] = (dir[2] - (1 - t1)*Mdir[2]) / t2;
                }
                else
                {
                    double t1 = FindBoundary(vsrc, dir, 1, 300);
                    double tmpPalette[3];
                    tmpPalette[0] = oldpalette_L[j];
                    tmpPalette[1] = oldpalette_A[j];
                    tmpPalette[2] = oldpalette_B[j];
                    double t2 = FindBoundary(tmpPalette, dir, 1, 300);

                    double lambda = min(t1 / t2, 1.0);
                    tdiff_L[j] = diffpalette_L[j];//* lambda;
                    tdiff_A[j] = diffpalette_A[j] * lambda;
                    tdiff_B[j] = diffpalette_B[j] * lambda;
                }
            }
            else
            {
                tdiff_L[j] = diffpalette_L[j];
                tdiff_A[j] = diffpalette_A[j];
                tdiff_B[j] = diffpalette_B[j];
            }

        }

        double* res = CalculateSinglePoint(param, palette_size, oldpalette_L, oldpalette_A, oldpalette_B, tdiff_L, tdiff_A, tdiff_B, vsrc);

        double* pc = LAB2RGB(res);
        grid_R[i] = pc[0];
        grid_G[i] = pc[1];
        grid_B[i] = pc[2];

        grid_R[i] = max(0.0, min(grid_R[i], 255.0));
        grid_G[i] = max(0.0, min(grid_G[i], 255.0));
        grid_B[i] = max(0.0, min(grid_B[i], 255.0));
    }
}

void Recoloring::drawRes(double*input_palette, double* output_palette, int palette_size)
{
    double* oldpalette_L = new double[palette_size + 1];
    double* oldpalette_A = new double[palette_size + 1];
    double* oldpalette_B = new double[palette_size + 1];
    double* newpalette_L = new double[palette_size + 1];
    double* newpalette_A = new double[palette_size + 1];
    double* newpalette_B = new double[palette_size + 1];
    for (int i = 0; i < palette_size; i++)
    {
        double* tmplab;
        double*tmp = new double[3];
        tmp[0] = input_palette[i * 3] * 255.0;
        tmp[1] = input_palette[i * 3 + 1] * 255.0;
        tmp[2] = input_palette[i * 3 + 2] * 255.0;
        tmplab = RGB2LAB(tmp);
        oldpalette_L[i] = tmplab[0]; oldpalette_A[i] = tmplab[1]; oldpalette_B[i] = tmplab[2];
        tmp[0] = output_palette[i * 3] * 255.0;
        tmp[1] = output_palette[i * 3 + 1] * 255.0;
        tmp[2] = output_palette[i * 3 + 2] * 255.0;
        tmplab = RGB2LAB(tmp);
        newpalette_L[i] = tmplab[0]; newpalette_A[i] = tmplab[1]; newpalette_B[i] = tmplab[2];
        delete[]tmplab;
        delete[]tmp;
    }

    oldpalette_L[palette_size] = 0;
    oldpalette_A[palette_size] = 0;
    oldpalette_B[palette_size] = 0;

    double* diffpalette_L = new double[palette_size + 1];
    double* diffpalette_A = new double[palette_size + 1];
    double* diffpalette_B = new double[palette_size + 1];

    for (int i = 0; i < palette_size; i++)
    {
        diffpalette_L[i] = newpalette_L[i] - oldpalette_L[i];
        diffpalette_A[i] = newpalette_A[i] - oldpalette_A[i];
        diffpalette_B[i] = newpalette_B[i] - oldpalette_B[i];
    }

    diffpalette_L[palette_size] = 0;
    diffpalette_A[palette_size] = 0;
    diffpalette_B[palette_size] = 0;


    this->CalculateGridResult(palette_size, oldpalette_L, oldpalette_A, oldpalette_B,
        diffpalette_L, diffpalette_A, diffpalette_B);

    int imageSize = this->imaging.cols*this->imaging.rows;
    for (int i = 0; i < imageSize; i++){

        int tmpR = 0;
        int tmpG = 0;
        int tmpB = 0;
        for (int k = 0; k < 8; k++){
            tmpR = tmpR + this->grid_R[this->weightindex[i][k]] * this->weightmap[i][k];
            tmpG = tmpG + this->grid_G[this->weightindex[i][k]] * this->weightmap[i][k];
            tmpB = tmpB + this->grid_B[this->weightindex[i][k]] * this->weightmap[i][k];
        }
        this->res_r[i] = tmpR;
        this->res_g[i] = tmpG;
        this->res_b[i] = tmpB;
    }
    drawImage();
}
void Recoloring::drawImage()
{
    int cols = imaging.cols;
    int rows = imaging.rows;
    int cnt = 0;
    for (int i = 0; i < rows; ++i)
        for (int j = 0; j < cols; ++j)
        {
            imaging.ptr<Vec3b>(i)[j].val[0] =
                this->res_r[cnt] * this->mask[cnt] + this->ori_b[cnt] * (1 - this->mask[cnt]);
            imaging.ptr<Vec3b>(i)[j].val[1] =
                this->res_g[cnt] * this->mask[cnt] + this->ori_g[cnt] * (1 - this->mask[cnt]);
            imaging.ptr<Vec3b>(i)[j].val[2] =
                this->res_b[cnt] * this->mask[cnt] + this->ori_r[cnt] * (1 - this->mask[cnt]);
            cnt = cnt + 1;
        }
}


double * Recoloring::k_means(Mat image, int center_num)
{
    Mat image_lab;
    int ngrid = 16;

    int grid_size = ngrid * ngrid * ngrid;

    double step_size = 255.0 / (ngrid - 1);
    double * sample_cnt = new double[grid_size];
    double **sample_sum = new double*[grid_size];
    for (int i = 0; i < grid_size; i++)
        sample_sum[i] = new double[3];
    for (int i = 0; i < grid_size; i++)
    {
        sample_cnt[i] = 0;
        sample_sum[i][0] = 0;
        sample_sum[i][1] = 0;
        sample_sum[i][2] = 0;
    }
    for (int i = 0; i < image.rows; i++) {
        for (int j = 0; j < image.cols; j++) {
            Vec3b pix_rgb = imaging.ptr<Vec3b>(i)[j];
            double rgb[3] = { pix_rgb.val[0], pix_rgb.val[1], pix_rgb.val[2] };
            double*lab = RGB2LAB(rgb);
            int bin1 = round(pix_rgb.val[0] / step_size);
            int bin2 = round(pix_rgb.val[1] / step_size);
            int bin3 = round(pix_rgb.val[2] / step_size);
            int bin = bin1 * ngrid * ngrid + bin2 * ngrid + bin3;
            sample_cnt[bin] = sample_cnt[bin] + 1;
            sample_sum[bin][0] = sample_sum[bin][0] + lab[0];
            sample_sum[bin][1] = sample_sum[bin][1] + lab[1];
            sample_sum[bin][2] = sample_sum[bin][2] + lab[2];
            delete[]lab;
        }
    }
    int tot = 0;
    for (int i = 0; i < grid_size; i++)
        if (sample_cnt[i]>0)
        {
            tot = tot + 1;
        }

    double **D = new double*[tot];
    for (int i = 0; i < tot; i++)
        D[i] = new double[3];
    tot = 0;
    for (int i = 0; i < grid_size; i++)
        if (sample_cnt[i]>0)
        {
            D[tot][0] = sample_sum[i][0] / sample_cnt[i];
            D[tot][1] = sample_sum[i][1] / sample_cnt[i];
            D[tot][2] = sample_sum[i][2] / sample_cnt[i];
            sample_cnt[tot] = sample_cnt[i];
            tot = tot + 1;
        }

    double **Center = new double*[center_num + 1];
    for (int i = 0; i < center_num + 1; i++)
        Center[i] = new double[3];

    double *pickcnt = new double[tot];
    for (int i = 0; i < tot; i++)
    {
        pickcnt[i] = sample_cnt[i];
    }

    for (int i = 0; i < center_num; i++)
    {
        int idx = 0;
        for (int j = 0; j<tot; j++)
            if (pickcnt[j] > pickcnt[idx])
            {
                idx = j;
            }

        Center[i] = D[idx];
        for (int j = 0; j < tot; j++)
        {
            double dis = 0;
            for (int k = 0; k<3; k++)
            {
                dis = dis + (D[idx][k] - D[j][k])*(D[idx][k] - D[j][k]);
            }
            dis = dis / (80.0 * 80.0);

            pickcnt[j] = pickcnt[j] * (1.0 - exp(-dis));
        }
    }

    //add black
    double black[3] = { 0.0, 0.0, 0.0 };

    Center[center_num][0] = RGB2LAB(black)[0];
    Center[center_num][1] = RGB2LAB(black)[1];
    Center[center_num][2] = RGB2LAB(black)[2];
    center_num = center_num + 1;

    double *cnt = new double[center_num];
    double **sumD = new double*[center_num];
    for (int i = 0; i < center_num; i++)
        sumD[i] = new double[3];

    for (int iter = 0; iter < 20; iter++)
    {
        for (int i = 0; i < center_num; i++)
        {
            sumD[i][0] = 0;
            sumD[i][1] = 0;
            sumD[i][2] = 0;
            cnt[i] = 0;
        }
        for (int i = 0; i < tot; i++)
        {
            int min_id = -1;
            double min_v = 1e100;
            for (int j = 0; j < center_num; j++)
            {
                double r = (D[i][0] - Center[j][0])*(D[i][0] - Center[j][0]) +
                    (D[i][1] - Center[j][1])*(D[i][1] - Center[j][1]) +
                    (D[i][2] - Center[j][2])*(D[i][2] - Center[j][2]);
                if (r < min_v)
                {
                    min_v = r;
                    min_id = j;
                }
            }

            cnt[min_id] = cnt[min_id] + sample_cnt[i];
            sumD[min_id][0] = sumD[min_id][0] + sample_cnt[i] * D[i][0];
            sumD[min_id][1] = sumD[min_id][1] + sample_cnt[i] * D[i][1];
            sumD[min_id][2] = sumD[min_id][2] + sample_cnt[i] * D[i][2];


        }

        for (int i = 0; i < center_num; i++)
            if (cnt[i]>0)
            {
                Center[i][0] = sumD[i][0] / cnt[i];
                Center[i][1] = sumD[i][1] / cnt[i];
                Center[i][2] = sumD[i][2] / cnt[i];
            }
    }

    center_num = center_num - 1;

    double *res = new double[3 * center_num];
    for (int i = 0; i < center_num; i++)
    {
        double ps[3];
        ps[0] = LAB2RGB(Center[i])[0];
        ps[1] = LAB2RGB(Center[i])[1];
        ps[2] = LAB2RGB(Center[i])[2];
        res[i * 3 + 0] = std::max(0.0, std::min(1.0, ps[0] / 255.0));
        res[i * 3 + 1] = std::max(0.0, std::min(1.0, ps[1] / 255.0));
        res[i * 3 + 2] = std::max(0.0, std::min(1.0, ps[2] / 255.0));
    }
    return res;
}

Mat Recoloring::test()
{
    double *testP = new double[15];
    for (int i = 0; i < 15; ++i)
    {
        testP[i] = this->center[i];
    }
    testP[12] = 0.3;
    testP[13] = 0.3;
    testP[14] = 0.3;
    this->drawRes(center, testP, 5);
    //cv::cvtColor(this->imaging, this->imaging, CV_RGB2BGR);
    return this->imaging;
}
double Recoloring::SmoothL(double x,double d)
{
    double lambda = 0.2 * log(2);
    return log(exp(lambda * x) + exp(lambda * d) - 1) / lambda - x;
}
void Recoloring::Shit(const QColor&color,int id)
{
    this->changed_center[id*3] = (double)color.red()/255.0;
    this->changed_center[id*3+1] = (double)color.green()/255.0;
    this->changed_center[id*3+2] = (double)color.blue()/255.0;
    this->drawRes(center, this->changed_center, 5);
    //cv::cvtColor(this->imaging, this->imaging, CV_RGB2BGR);
    emit sendShit(this->imaging);
}
